#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include "bmp085.h"

#include "../drv/drv_timer.h"
#include "../drv/drv_i2c.h"


#define BMP085ADDRESS 0x77 

const unsigned int OSS = 0;  // Oversampling Setting

// Calibration values
int ac1;
int ac2; 
int ac3; 
unsigned int ac4;
unsigned int ac5;
unsigned int ac6;
int b1; 
int b2;
int mb;
int mc;
int md;

long b5; 

//long ut;

long m_fd = -1;;
// Use these for altitude conversions
const float p0 = 101325;     // Pressure at sea level (Pa)


static long BMP085ReadUP( int fd );
static void BMP085ReadUT( int fd );
// Calculate altitude
static void BMP085GetAltitude( int fd , S_Baro *baro );
// Calculate temperature
static void BMP085GetTemperature( int fd , S_Baro *baro );
// Calculate pressure
static void BMP085GetPressure( int fd , S_Baro *baro );


//Init
void BMP085Init( int fd )
{
	printf("BMP085 Init\n");
	m_fd = fd;

}

// Calculate pressure temperature altitude
void BMP085Read( int fd , S_Baro *baro )
{
	BMP085GetTemperature( fd , baro );
	BMP085GetPressure( fd , baro );
	BMP085GetAltitude( fd, baro );
}

// Calculate Calibrate
void BMP085Calibrate( int fd )
{
	printf("BMP085 Calibrate...");
	fflush(stdout);
	drv_i2cSetDevice(fd, BMP085ADDRESS);
	ac1 = drv_i2cReadShort(fd, 0xAA);
        if(ac1>32768)
	{
		ac1=ac1-65535;
	}
	ac2 = drv_i2cReadShort(fd, 0xAC);
        if(ac2>32768)
	{
		ac2=ac2-65535;
	}
	ac3 = drv_i2cReadShort(fd, 0xAE);
        if(ac3>32768)
	{
		ac3=ac3-65535;
	}
	ac4 = drv_i2cReadShort(fd, 0xB0);
	ac5 = drv_i2cReadShort(fd, 0xB2);
	ac6 = drv_i2cReadShort(fd, 0xB4);
	b1 = drv_i2cReadShort(fd, 0xB6);
	if(b1>32768)
	{
		b1=b1-65535;
	}
	b2 = drv_i2cReadShort(fd, 0xB8);
	if(b2>32768)
	{
		b2=b2-65535;
	}
	mb = drv_i2cReadShort(fd, 0xBA);
	if(mb>32768)
	{
		mb=mb-65535;
	}
	mc = drv_i2cReadShort(fd, 0xBC);
	if(mc>32768)
	{
		mc=mc-65535;
	}
	md = drv_i2cReadShort(fd, 0xBE);
	if(md>32768)
	{
		md=md-65535;
	}

	printf("\rBMP085 Calibrate   \n");
}

// Calculate Altitude
static void BMP085GetAltitude( int fd , S_Baro *baro )
{
	drv_i2cSetDevice(fd, BMP085ADDRESS);
	baro->altitude = (float)44330 * (float)((float)1 - pow(((float) baro->pressure/(float)p0), (float)0.190295));
}

// Calculate temperature
static void BMP085GetTemperature( int fd , S_Baro *baro )
{
	BMP085ReadUT( fd );
	baro->temperature = ((b5 + 8)>>4);
}

// Calculate pressure
static void BMP085GetPressure( int fd , S_Baro *baro )
{
	long x1, x2, x3, b3, b6, p;
	unsigned long b4, b7;

	drv_i2cSetDevice(fd, BMP085ADDRESS);
	long up = BMP085ReadUP( fd );
	b6 = b5 - 4000;
	
	// Calculate B3
	x1 = (b2 * (b6 * b6)>>12)>>11;
	x2 = (ac2 * b6)>>11;
	x3 = x1 + x2;
	b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;

	// Calculate B4
	x1 = (ac3 * b6)>>13;
	x2 = (b1 * ((b6 * b6)>>12))>>16;
	x3 = ((x1 + x2) + 2)>>2;
	b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;

	b7 = ((unsigned long)(up - b3) * (50000>>OSS));
	if (b7 < 0x80000000)
	{
		p = (b7<<1)/b4;
	}
	else
	{
		p = (b7/b4)<<1;
	}

	x1 = (p>>8) * (p>>8);
	x1 = (x1 * 3038)>>16;
	x2 = (-7357 * p)>>16;
	p += (x1 + x2 + 3791)>>4;

	baro->pressure = p;
}

// Read the uncompensated temperature value
static void BMP085ReadUT( int fd )
{
	long x1, x2;
  	// This requests a temperature reading
	drv_i2cSetDevice(fd, BMP085ADDRESS);
	drv_i2cWriteByte(fd, 0xF4, 0x2E);

	// Wait at least 4.5ms;
	usleep(5000);

	// Read two bytes from registers 0xF6 and 0xF7
	drv_i2cSetDevice(m_fd, BMP085ADDRESS);
	long ut = drv_i2cReadShort(m_fd, 0xF6);
	x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
	x2 = ((long)mc << 11)/(x1 + md);
	b5 = x1 + x2;

	/*printf("drv_timerAdd(0, 450 , BMP085Handler, ONE_SHOT_TIME)\n");
	fflush(stdout);
	drv_timerAdd(0, 450 , BMP085Handler, ONE_SHOT_TIMER);*/
}

// Read the uncompensated pressure value
static long BMP085ReadUP( int fd )
{
	unsigned int msb, lsb, xlsb;
	long up = 0;

	// Write 0x34+(OSS<<6) into register 0xF4
	// Request a pressure reading w/ oversampling setting
	drv_i2cSetDevice(fd, BMP085ADDRESS);
	drv_i2cWriteByte(fd, 0xF4, 0x34 + (OSS<<6));
	
	// Wait for conversion, delay time dependent on OSS
	usleep(2000 + (3<<OSS)*1000);

	// Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)
	msb = drv_i2cReadByte(fd, 0xF6);
	lsb = drv_i2cReadByte(fd, 0xF7);
	xlsb = drv_i2cReadByte(fd, 0xF8);

	up = (((long) msb << 16) | ((long) lsb << 8) | (long) xlsb) >> (8-OSS);

	return up;
}


					



